#include "interpolationHelper.h"
#include "skeletalActor.h"
#include "renderer.h"

InterpolationHelper::InterpolationHelper(){

    renderer=Renderer::getInstance();
    bFinished=false;
    bRelative=false;
    bAdditive=false;
    startTime=0.0f;
    currentTime=0;
    currentKey=0;


    bInterpolateVectors=false;
    bInterpolateMatrix=false;
    bInterpolateProperty=false;
}

InterpolationHelper::~InterpolationHelper(){
}

void InterpolationHelper::interpolate(){

    if (!bFinished && keyFrames.size()>1){

        if (bInterpolateVectors)
                interpolateVectors();

        if (bInterpolateMatrix)
                interpolateMatrix();
    }
}

void InterpolationHelper::interpolateVectors(){

    Vector3f locationOne, locationTwo;
    Vector3f rotationOne, rotationTwo;

    Vector3f resultingLocation, resultingRotation;

    double timeKeyOne;
    double timeKeyTwo;

    currentTime=renderer->currentTime-startTime;

    //move forward if we are at end of key
    while (currentTime> keyFrames[currentKey+1]->timeKey ){
    //if (currentTime> keyFrames[currentKey+1]->timeKey ){
          currentKey++;
          if ((uint)currentKey+1 >= keyFrames.size()){
            bFinished=true;
            return;
            }
          }

    timeKeyOne=keyFrames[currentKey]->timeKey;
    timeKeyTwo=keyFrames[currentKey+1]->timeKey;

    //get the time difference
    double timeDifference=timeKeyTwo-timeKeyOne;

    //map time difference to 0.0 - 1.0
    double keyTime=currentTime-timeKeyOne; //-> current Position, in the beginning 0.0;
    float relativeTime=(float) (keyTime/timeDifference); //-> will go from 0.0 to 1.0 between the keys


    if (bAdditive){
        locationOne=moveActor->location;                //
        rotationOne=moveActor->rotation;
        relativeTime*=relativeTime;      //
    }else{
        locationOne=keyFrames[currentKey]->locationKey;
        rotationOne=keyFrames[currentKey]->rotationKey;
    }


    locationTwo=keyFrames[currentKey+1]->locationKey;
    rotationTwo=keyFrames[currentKey+1]->rotationKey;

    //interpolate between them
    resultingLocation=locationOne.lerp(relativeTime,locationTwo); //calculate resulting position
    resultingRotation=rotationOne.lerp(relativeTime,rotationTwo); //calculate resulting position

    //apply resulting position
    if (bRelative){
        moveActor->setLocation(resultingLocation+baseLocation);
        moveActor->setRotation(resultingRotation+baseRotation);
        }
    else{
        moveActor->setLocation(resultingLocation);
        moveActor->setRotation(resultingRotation);
        }
}

void InterpolationHelper::interpolateMatrix(){

    Matrix4f matrixOne;
    Matrix4f matrixTwo;

    Matrix4f resultingMatrix;

    double timeKeyOne;
    double timeKeyTwo;

    currentTime=renderer->currentTime-startTime;

    //currentTime=startTime+1;
    //currentTime++;
    //move forward if we are at end of key

    while (currentTime> keyFrames[currentKey+1]->timeKey ){
          currentKey++;
          if ((uint)currentKey+1 >= keyFrames.size()){
            bFinished=true;
            return;
            }
          }

    //cout << "key: " << currentKey << " max Keys: " << (*timeKeys).size() << endl;
    //if we are at the end of all keys, we're finished!

    //get the two closest interpolation points
    timeKeyOne=keyFrames[currentKey]->timeKey;
    timeKeyTwo=keyFrames[currentKey+1]->timeKey;

        //get the time difference
        double timeDifference=timeKeyTwo-timeKeyOne;

        //map time difference to 0.0 - 1.0
        double keyTime=currentTime-timeKeyOne; //-> current Position, in the beginning 0.0;
        float relativeTime=(float) (keyTime/timeDifference); //-> will go from 0.0 to 1.0 between the keys

    SkeletalActor* skel=(SkeletalActor*)moveActor;

    for (uint i=0;i<skel->bones.size();i++){

        matrixOne=keyFrames[currentKey]->boneMatrices[skel->bones[i]->name];
        matrixTwo=keyFrames[currentKey+1]->boneMatrices[skel->bones[i]->name];

        //interpolate between them
        resultingMatrix=matrixOne.lerp(relativeTime,matrixTwo); //calculate resulting position

        //apply resulting position
        if (bRelative)
            skel->bones[i]->transformMatrix=resultingMatrix * baseBoneMatrices[skel->bones[i]->name];
        else
            skel->bones[i]->transformMatrix=resultingMatrix;
        }
}

